
#include "cubic_bspline_curve_cuda.h"
#include "cubic_bspline_curve.h"

namespace shermit {
#define DCurve BSplineCurve<Float_t, 3, DeviceType::GPU>

	template<typename Float_t>
	GENERAL DCurve::DCurve() {
		control_pts = nullptr;
		num_control_pts = 0;
	}

	template<typename Float_t>
	GENERAL inline int DCurve::degree() const {
		return 3;
	}

	template<typename Float_t>
	GENERAL inline int DCurve::nControlPts() const {
		return num_control_pts;
	}

	template<typename Float_t>
	GENERAL inline int DCurve::nKnots() const {
		return nControlPts() + degree() + 1;
	}

	template<typename Float_t>
	const DCurve::vec_t* DCurve::getControlPts() const {
		return control_pts;
	}

	template<typename Float_t>
	GENERAL inline Float_t DCurve::tMin() const {
		return (Float_t)degree();
	}

	template<typename Float_t>
	GENERAL inline Float_t DCurve::tMax() const {
		return (Float_t)nControlPts();
	}

	template<typename Float_t>
	__device__ Float_t DCurve::baseFuncVal(int i, Float_t t) const {
		t = t - i;
		if (t < 0 || t>4) return 0;
		if (t < 1) return t * t * t / 6;
		if (1 <= t && t < 2) {
			Float_t t2 = t * t;
			return (-3.0 * t2 * t + 12 * t2 - 12 * t + 4) / 6;
		}
		if (2 <= t && t < 3) {
			Float_t t2 = t * t;
			return (3.0 * t2 * t - 24 * t2 + 60 * t - 44) / 6;
		}
		// [3,4)
		Float_t it = 4 - t;
		return it * it * it / 6;
	}

	template<typename Float_t>
	__device__ DCurve::vec_t DCurve::at(Float_t t) const {
		int k = t;
		Float_t t0 = t - (k - degree());
		Float_t it0 = 4 - t0;
		Float_t b0 = it0 * it0 * it0 / 6;
		Float_t t1 = t0 - 1;
		Float_t it1 = 4 - t1;
		Float_t b1 = (-3 * it1 * it1 * it1 + 12 * it1 * it1 - 12 * it1 + 4) / 6;
		Float_t t2 = t1 - 1;
		Float_t b2 = (-3 * t2 * t2 * t2 + 12 * t2 * t2 - 12 * t2 + 4) / 6;
		Float_t t3 = t2 - 1;
		Float_t b3 = t3 * t3 * t3 / 6;

		// calculate the curve point
		return
			control_pts[k - 3] * b0 +
			control_pts[k - 2] * b1 +
			control_pts[k - 1] * b2 +
			control_pts[k - 0] * b3;
	}

	template<typename Float_t>
	__device__ DCurve::vec_t DCurve::df(Float_t t, unsigned int order) const {
		switch (order) {
		case 0:
			return at(t);
		case 1:
			return df1(t);
		default:
			return df2(t);  // df3 not implemented
		}
	}

	template<typename Float_t>
	__device__ inline DCurve::vec_t DCurve::df1(Float_t t) const {
		int k = t;
		Float_t t0 = t - (k - degree());
		Float_t t0_2 = t0 * t0;
		Float_t db0 = -(t0 - 4) * (t0 - 4) / 2;
		Float_t db1 = (9 * t0_2 - 66 * t0 + 117) / 6;
		Float_t db2 = (-9 * t0_2 + 60 * t0 - 96) / 6;
		Float_t db3 = (t0 - 3) * (t0 - 3) / 2;

		return
			control_pts[k - 3] * db0 +
			control_pts[k - 2] * db1 +
			control_pts[k - 1] * db2 +
			control_pts[k - 0] * db3;
	}

	template<typename Float_t>
	__device__ inline DCurve::vec_t DCurve::df2(Float_t t) const {
		int k = t;
		Float_t t0 = t - (k - degree());
		Float_t d2_b0 = 4 - t0;
		Float_t d2_b1 = 3 * t0 - 11;
		Float_t d2_b2 = -3 * t0 + 10;
		Float_t d2_b3 = t0 - 3;

		return
			control_pts[k - 3] * d2_b0 +
			control_pts[k - 2] * d2_b1 +
			control_pts[k - 1] * d2_b2 +
			control_pts[k - 0] * d2_b3;
	}

	template<typename Float_t>
	__device__ DCurve::vec_t DCurve::tangent(Float_t t) const {
		return nv::normalize(df1(t));
	}

	/* direction towards the curvature circle */
	template<typename Float_t>
	__device__ DCurve::vec_t DCurve::normal(Float_t t) const {
		vec_t tgt = tangent(t);
		vec_t d2_p = df2(t);
		Float_t sign = (tgt[0] * d2_p[1] - tgt[1] * d2_p[0]); // sign of curvature
		if (sign > 0) return vec_t(-tgt.y, tgt.x);
		return vec_t(tgt.y, -tgt.x);
	}

	template<typename Float_t>
	__device__ Float_t DCurve::curvature(Float_t t) const {
		vec_t d1_p = df1(t);
		vec_t d2_p = df2(t);
		Float_t nmt = abs(d1_p[0] * d2_p[1] - d1_p[1] * d2_p[0]);
		Float_t dmt = nv::length(d1_p);
		dmt = dmt * dmt * dmt;
		return nmt / dmt;
	}

	template<typename Float_t>
	void DCurve::copyToHost(BSplineCurve<Float_t, 3, DeviceType::CPU>& h_curve) const {
		h_curve.deallocate();
		h_curve.setControlPts(this->control_pts, this->num_control_pts, DeviceType::GPU);
	}

	template<typename Float_t>
	void DCurve::setControlPts(const vec_t* pts, int num_pts, DeviceType from) {
		if (!pts || num_pts <= 0) return;
		if (num_control_pts != num_pts) {
			deallocate();
			num_control_pts = num_pts;
		}
		printf("num of control pts: %d\n", num_pts);
		checkCudaErrors(GPUMalloc(control_pts, sizeof(vec_t) * num_pts));
		if (from == DeviceType::CPU) {
			checkCudaErrors(CPU2GPU(control_pts, pts, sizeof(vec_t) * num_pts));
		}
		else {
			checkCudaErrors(GPU2GPU(control_pts, pts, sizeof(vec_t) * num_pts));
		}
		return;
	}

	template<typename Float_t>
	void DCurve::deallocate() {
		checkCudaErrors(cudaFree(control_pts));
		control_pts = nullptr;
		num_control_pts = 0;
	}

	template class BSplineCurve<float, 3, DeviceType::GPU>;
	template class BSplineCurve<double, 3, DeviceType::GPU>;
}
